/**
 * This code was written by Derek Nedelman and released at www.gameprojects.com
 * 
 * Derek Nedelman and GameProjects.com assume no responsibility for any harm caused 
 * by using this code.
 * 
 * This code is in the public domain. That said, it would be nice if you left 
 * the previous comments if you decide to use it for anything. 
 */

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;

namespace Xfinity.Engine
{
    /// <summary>
    /// A collection of useful math functions.
    /// </summary>
    public static class MathUtilities
    {
        #region Scalar functions

        /// <summary>
        /// Gets the square root of the specified value. This is an alias
        /// for (float)Math.Sqrt()
        /// </summary>        
        public static float Sqrt(float value)
        {
            return (float)Math.Sqrt(value);
        }

        /// <summary>
        /// Gets the sine of the specified value. This is an alias
        /// for (float)Math.Sin()
        /// </summary>
        public static float Sin(float value)
        {
            return (float)Math.Sin(value);
        }

        /// <summary>
        /// Gets the cosine of the specified value. This is an alias
        /// for (float)Math.Cos()
        /// </summary>
        public static float Cos(float value)
        {
            return (float)Math.Cos(value);
        }

        /// <summary>
        /// Gets the sine and cosine of the specified value. 
        /// </summary>
        /// <param name="value">The value for which the sine and cosine is obtained</param>
        /// <param name="sin">The resultant sine value</param>
        /// <param name="cos">The resultant cosine value</param>
        public static void SinCos(float value, out float sin, out float cos)
        {
            sin = (float)Math.Sin(value);
            cos = (float)Math.Cos(value);
        }

        /// <summary>
        /// Gets sign of the specified value
        /// </summary>
        /// <param name="value">The value for which the sign is obtained</param>
        /// <returns>
        /// -1 is returned if value is less than 0. Otherwise 1 is returned.
        /// </returns>
        public static float Sign(float value)
        {
            if (value < 0.0f)
                return -1.0f;
            else
                return 1.0f;
        }

        /// <summary>
        /// Gets sign of the specified value
        /// </summary>
        /// <param name="value">The value for which the sign is obtained</param>
        /// <returns>
        /// -1 is returned if value is less than 0. Otherwise 1 is returned.
        /// </returns>
        public static int Sign(int value)
        {
            if (value < 0)
                return -1;
            else
                return 1;
        }

        public static bool AlmostZero(float f)
        {
            return Math.Abs(f) < .0000001f;
        }

        #endregion 

        #region Vector functions

        /// <summary>
        /// Calculates the dot product of two vectors.
        /// </summary>
        /// <param name="v0">A vector</param>
        /// <param name="v1">Another vector</param>
        /// <returns>The dot product of the two vectors</returns>
        public static float Dot(ref Vector3 v0, ref Vector3 v1)
        {
            return (v0.X * v1.X) + (v0.Y * v1.Y) + (v0.Z * v1.Z);
        }

        /// <summary>
        /// Gets the maximum of the vector's components
        /// </summary>
        /// <param name="v">The vector</param>
        /// <returns>The maximum of the vector's components</returns>
        public static float Max(ref Vector3 v)
        {
            return Math.Max(v.X, Math.Max(v.Y, v.Z));
        }

        /// <summary>
        /// Creates a normal for the specified triangle points
        /// </summary>
        /// <param name="p0">The first triangle point</param>
        /// <param name="p1">The second triangle point</param>
        /// <param name="p2">The third triangle point</param>
        /// <param name="result">The created normal</param>
        public static void CreateNormal(ref Vector3 p0, ref Vector3 p1, ref Vector3 p2, out Vector3 result)
        {
            Vector3 v0;
            Vector3.Subtract(ref p1, ref p0, out v0);

            Vector3 v1;
            Vector3.Subtract(ref p2, ref p1, out v1);

            Vector3.Cross(ref v0, ref v1, out result);
            result.Normalize();
        }

        /// <summary>
        /// Calculates the reflection of the vector about the specified normal
        /// </summary>
        /// <param name="v">The vector for which the reflected result is obtained</param>
        /// <param name="normal">The normal to reflect about</param>
        /// <returns>The reflected vector</returns>
        public static Vector3 Reflected(Vector3 v, Vector3 normal)
        {
            return normal * (2.0f * Dot(ref v, ref normal)) - v;
        }

        public static void Reflected(ref Vector3 v, ref Vector3 normal, out Vector3 result)
        {
            result = normal * (2.0f * Dot(ref v, ref normal)) - v;
        }

        /// <summary>
        /// Calculates the projection of a vector onto a normal.
        /// </summary>
        /// <param name="v">The vector to project</param>
        /// <param name="normal">
        /// The normal onto which the vector is projected. This vector is
        /// assumed to be of unit length
        /// </param>
        /// <returns></returns>
        public static Vector3 ProjectionOntoNormal(Vector3 v, Vector3 normal)
        {
            return v * Dot(ref v, ref normal);
        }

        public static void ProjectionOntoNormal(ref Vector3 v, ref Vector3 normal, out Vector3 result)
        {
            result = v * Dot(ref v, ref normal);
        }

        /// <summary>
        /// Projects a vector onto another
        /// </summary>
        /// <param name="v0">The vector to project</param>
        /// <param name="v1">
        /// The vector onto which the projection occurs. This vector does not need 
        /// to be of unit length
        /// </param>
        /// <returns>The projection of v0 onto v1</returns>
        public static Vector3 ProjectionOntoVector(Vector3 v0, Vector3 v1)
        {
            Vector3 result;
            ProjectionOntoVector(ref v0, ref v1, out result);
            return result;
        }

        public static void ProjectionOntoVector(ref Vector3 v0, ref Vector3 v1, out Vector3 result)
        {
            result = v1 * (Dot(ref v0, ref v1) / Dot(ref v1, ref v1));
        }

        /// <summary>
        /// Multiplies all of the vectors in an array by the specified matrix
        /// </summary>
        /// <param name="m">The matrix</param>
        /// <param name="v">
        /// The array of vectors to multiply. The transformed results overwrite 
        /// the original values.
        /// </param>
        public static void TransformArray(Vector3[] v, ref Matrix m)
        {
            Vector3 temp;
            for (int index = 0; index < v.Length; index++)
            {
                Vector3.Transform(ref v[index], ref m, out temp);
                v[index] = temp;
            }
        }

        /// <summary>
        /// Multiplies a vector by the specified matrix's rotation submatrix
        /// </summary>
        /// <param name="m">The matrix</param>
        /// <param name="v">The vector</param>
        /// <returns>The multiplied vector</returns>
        public static void TransformRotation(ref Vector3 v, ref Matrix m, out Vector3 result)
        {
            result = new Vector3
                (
                v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31,
                v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32,
                v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33
                );
        }

        /// <summary>
        /// Multiplies a vector by the specified projection matrix, and returns 
        /// the projected result, which is obtained by dividing the elements
        /// by the hypothetical 'w' component
        /// </summary>
        /// <param name="m">The projection matrix</param>
        /// <param name="v">The vector</param>
        /// <returns>The transformed, projected vector</returns>
        public static void ProjectVector3(ref Vector3 v, ref Matrix m, out Vector3 result)
        {
            result = new Vector3
                (
                v.X * m.M11 + v.Y * m.M21 + v.Z * m.M31 + m.M41,
                v.X * m.M12 + v.Y * m.M22 + v.Z * m.M32 + m.M42,
                v.X * m.M13 + v.Y * m.M23 + v.Z * m.M33 + m.M43
                );

            float ooW = 1.0f / ((m.M14 * v.X) + (m.M24 * v.Y) + (m.M34 * v.Z) + m.M44);
            result.X *= ooW;
            result.Y *= ooW;
            result.Z *= ooW;
        }

        /// <summary>
        /// Gets the squared distance between two points
        /// </summary>
        /// <param name="p0">The first point</param>
        /// <param name="p1">The second point</param>        
        public static float SquaredDistance(ref Vector3 p0, ref Vector3 p1)
        {
            Vector3 offset;
            Vector3.Subtract(ref p0, ref p1, out offset);
            return offset.LengthSquared();
        }

        #endregion

        #region Matrix functions

        /// <summary>
        /// Creates a matrix from row vectors
        /// </summary>
        /// <param name="m0">Row 1</param>
        /// <param name="m1">Row 2</param>
        /// <param name="m2">Row 3</param>
        /// <param name="m3">Row 4</param>
        /// <returns></returns>
        public static Matrix MatrixFromRowVectors(Vector3 m0, Vector3 m1, Vector3 m2, Vector3 m3)
        {
            return new Matrix
                (
                m0.X, m0.Y, m0.Z, 0.0f,
                m1.X, m1.Y, m1.Z, 0.0f,
                m2.X, m2.Y, m2.Z, 0.0f,
                m3.X, m3.Y, m3.Z, 1.0f
                );
        }

        public static void MatrixFromRowVectors
            (
            ref Vector3 m0,
            ref Vector3 m1,
            ref Vector3 m2,
            ref Vector3 m3,
            out Matrix result
            )
        {
            result = new Matrix
                (
                m0.X, m0.Y, m0.Z, 0.0f,
                m1.X, m1.Y, m1.Z, 0.0f,
                m2.X, m2.Y, m2.Z, 0.0f,
                m3.X, m3.Y, m3.Z, 1.0f
                );
        }

        /// <summary>
        /// Gets the rotation submatrix of the specified matrix
        /// </summary>
        /// <param name="m">The matrix for which the rotation submatrix is obtained</param>
        /// <returns>The rotation submatrix</returns>
        public static Matrix GetRotationMatrix(Matrix m)
        {
            return new Matrix
                (
                m.M11, m.M12, m.M13, 0,
                m.M21, m.M22, m.M23, 0,
                m.M31, m.M32, m.M33, 0,
                0, 0, 0, 1
                );
        }

        public static void GetRotationMatrix(ref Matrix m, out Matrix result)
        {
            result = new Matrix
                (
                m.M11, m.M12, m.M13, 0,
                m.M21, m.M22, m.M23, 0,
                m.M31, m.M32, m.M33, 0,
                0, 0, 0, 1
                );
        }

        /// <summary>
        /// Gets the specified row vector from a matrix
        /// </summary>
        /// <param name="m">The matrix for which a row vector is obtained</param>
        /// <param name="row">Zero-based index of the row to get</param>
        /// <param name="result">The result</param>
        /// <returns>The specified row vector</returns>
        public static void GetRowVector3(ref Matrix m, int row, out Vector3 result)
        {
            switch (row)
            {
                case 0: result = new Vector3(m.M11, m.M12, m.M13); break;
                case 1: result = new Vector3(m.M21, m.M22, m.M23); break;
                case 2: result = new Vector3(m.M31, m.M32, m.M33); break;
                case 3: result = new Vector3(m.M41, m.M42, m.M43); break;
                default: throw new ArgumentException("Invalid row index: " + row);
            }
        }

        /// <summary>
        /// Sets the specified row vector of a matrix
        /// </summary>
        /// <param name="m">The matrix for which a row vector is set</param>
        /// <param name="row">Zero-based index of the row to set</param>
        /// <param name="v">The vector of values to set</param>        
        public static void SetRowVector3(ref Matrix m, int row, ref Vector3 v)
        {
            switch (row)
            {
                case 0: m.M11 = v.X; m.M12 = v.Y; m.M13 = v.Z; break;
                case 1: m.M21 = v.X; m.M22 = v.Y; m.M23 = v.Z; break;
                case 2: m.M31 = v.X; m.M32 = v.Y; m.M33 = v.Z; break;
                case 3: m.M41 = v.X; m.M42 = v.Y; m.M43 = v.Z; break;
                default: throw new ArgumentException("Invalid row index: " + row);
            }
        }

        /// <summary>
        /// Gets the specified column vector from a matrix
        /// </summary>
        /// <param name="m">The matrix for which a column vector is obtained</param>
        /// <param name="row">Zero-based index of the column to get</param>
        /// <returns>The specified column vector</returns>
        public static void GetColumnVector3(ref Matrix m, int row, out Vector3 result)
        {
            switch (row)
            {
                case 0: result = new Vector3(m.M11, m.M21, m.M31); break;
                case 1: result = new Vector3(m.M12, m.M22, m.M32); break;
                case 2: result = new Vector3(m.M13, m.M23, m.M33); break;
                case 3: result = new Vector3(m.M14, m.M24, m.M34); break;
                default: throw new ArgumentException("Invalid column index: " + row);
            }
        }

        /// <summary>
        /// Transposes the rotation submatrix of the specified matrix
        /// </summary>
        /// <param name="m">The matrix</param>
        /// <returns>A matrix with the transposed rotation submatrix of 'm'.</returns>
        public static Matrix TransposedRotation(Matrix m)
        {
            return new Matrix
                (
                m.M11, m.M21, m.M31, 0,
                m.M12, m.M22, m.M32, 0,
                m.M13, m.M23, m.M33, 0,
                0, 0, 0, 1
                );
        }

        public static void TransposedRotation(ref Matrix m, out Matrix result)
        {
            result = new Matrix
                (
                m.M11, m.M21, m.M31, 0,
                m.M12, m.M22, m.M32, 0,
                m.M13, m.M23, m.M33, 0,
                0, 0, 0, 1
                );
        }

        /// <summary>
        /// Inverts the specified matrix, treating it as if it were orthonormal. This
        /// is typically used for matrices composed of translations and rotations
        /// </summary>
        /// <param name="m">The orthonormal matrix to invert</param>
        /// <returns>A new, inverted matrix</returns>
        public static Matrix InvertRotationTranslationMatrix(Matrix m)
        {
            Matrix inverse;
            InvertRotationTranslationMatrix(ref m, out inverse);
            return inverse;
        }

        /// <summary>
        /// Inverts a matrix that is assumed to be composed of only rotations and
        /// translations. Such matrices are extremely easy to invert. 
        /// </summary>
        /// <param name="m">The matrix to invert</param>
        /// <param name="result">The inverted matrix result</param>
        public static void InvertRotationTranslationMatrix(ref Matrix m, out Matrix result)
        {
            //Transpose rotations (inverse = [Rtranspose])
            TransposedRotation(ref m, out result);

            //Invert translations (inverse = [-T][Rtranspose])
            result.M41 = -(m.M41 * m.M11 + m.M42 * m.M12 + m.M43 * m.M13);
            result.M42 = -(m.M41 * m.M21 + m.M42 * m.M22 + m.M43 * m.M23);
            result.M43 = -(m.M41 * m.M31 + m.M42 * m.M32 + m.M43 * m.M33);
        }

        /// <summary>
        /// Inverts just the translations of a matrix that is assumed to be 
        /// composed of only rotations and translations.
        /// </summary>
        /// <param name="m">The matrix whose translations are to be inverted</param>
        /// <param name="result">The inverted translation result</param>
        public static void InvertRotationTranslationMatrixTranslations(ref Matrix m, out Vector3 result)
        {
            result = new Vector3
                (
                -(m.M41 * m.M11 + m.M42 * m.M12 + m.M43 * m.M13),
                -(m.M41 * m.M21 + m.M42 * m.M22 + m.M43 * m.M23),
                -(m.M41 * m.M31 + m.M42 * m.M32 + m.M43 * m.M33)
                );
        }

        /// <summary>
        /// Inverts just the translations of a matrix that is assumed to be 
        /// composed of only rotations and translations. The results are placed into
        /// the 4th row of the specified 'result' matrix
        /// </summary>
        /// <param name="m">The matrix whose translations are to be inverted</param>
        /// <param name="result">
        /// The matrix into which the inverted translation result is placed
        /// </param>
        public static void InvertRotationTranslationMatrixTranslations(ref Matrix m, ref Matrix result)
        {
            result.M41 = -(m.M41 * m.M11 + m.M42 * m.M12 + m.M43 * m.M13);
            result.M42 = -(m.M41 * m.M21 + m.M42 * m.M22 + m.M43 * m.M23);
            result.M43 = -(m.M41 * m.M31 + m.M42 * m.M32 + m.M43 * m.M33);
        }

        /// <summary>
        /// Creates a matrix that is the transpose(inverse(m)) of the specified matrix.
        /// The returned matrix is typically used to transform normals
        /// </summary>
        /// <param name="m"></param>
        /// <returns></returns>
        public static Matrix InverseTranspose(Matrix m)
        {
            Matrix result;
            InverseTranspose(ref m, out result);
            return result;
        }

        public static void InverseTranspose(ref Matrix m, out Matrix result)
        {
            Matrix inverse;
            Matrix.Invert(ref m, out inverse);
            Matrix.Transpose(ref inverse, out result);
        }

        /// <summary>
        /// Normalizes the rows of the rotation submatrix
        /// </summary>
        /// <param name="m">The matrix to normalize</param>
        public static void Normalize(ref Matrix m)
        {
            float row1Normalizer = 1.0f / Sqrt(m.M11 * m.M11 + m.M12 * m.M12 + m.M13 * m.M13);
            m.M11 *= row1Normalizer; m.M12 *= row1Normalizer; m.M13 *= row1Normalizer;

            float row2Normalizer = 1.0f / Sqrt(m.M21 * m.M21 + m.M22 * m.M22 + m.M23 * m.M23);
            m.M21 *= row2Normalizer; m.M22 *= row2Normalizer; m.M23 *= row2Normalizer;

            float row3Normalizer = 1.0f / Sqrt(m.M31 * m.M31 + m.M32 * m.M32 + m.M33 * m.M33);
            m.M31 *= row3Normalizer; m.M32 *= row3Normalizer; m.M33 *= row3Normalizer;
        }

        /// <summary>
        /// Normalizes the rows of the rotation submatrix
        /// </summary>
        /// <param name="m">The matrix to normalize</param>
        /// <returns>A new, normalized version of the input matrix</returns>
        public static Matrix Normalized(Matrix m)
        {
            Normalize(ref m);
            return m;
        }

        public static void OrthonormalizeRotations(ref Matrix m, int startRow)
        {
            Vector3 v0, v1, v2;	  //Perpendicular vectors generated from orthogonalization process

            int nextRow = (startRow + 1) % 3;
            int nextNextRow = (startRow + 2) % 3;

            Vector3 c0;
            GetRowVector3(ref m, startRow, out c0);

            Vector3 c1;
            GetRowVector3(ref m, nextRow, out c1);

            Vector3 c2;
            GetRowVector3(ref m, nextNextRow, out c2);

            //First basis vector
            //v0 = c0
            v0 = c0;

            //Second basis vector	
            //v1 = c1 - c1_on_v0
            Vector3 c1_on_v0;
            ProjectionOntoVector(ref c1, ref v0, out c1_on_v0); //c1 projected onto c0			
            v1 = c1 - c1_on_v0;

            //Third basis vector
            //v2 = c2 - c2_on_v0 - c2_on_v1 = c2 - (c2_on_v0 + c2_on_v1)
            Vector3 c2_on_v0;
            ProjectionOntoVector(ref c2, ref v0, out c2_on_v0); //c2 projected onto v0

            Vector3 c2_on_v1;
            ProjectionOntoVector(ref c2, ref v1, out c2_on_v1); //c2 projected onto v1
            v2 = c2 - (c2_on_v0 + c2_on_v1);

            //Normalize the rows
            v0.Normalize();
            v1.Normalize();
            v2.Normalize();

            //Set the matrix
            SetRowVector3(ref m, startRow, ref v0);
            SetRowVector3(ref m, nextRow, ref v1);
            SetRowVector3(ref m, nextNextRow, ref v2);
        }

        /// <summary>
        /// Orthonormalizes the rotation submatrix of a matrix using the Gram-Schmidt process
        /// </summary>
        /// <param name="m">The matrix to orthonormalize</param>
        /// <returns>
        /// Returns the input matrix with its rotation rows orthonormalized. The
        /// translations are left intact.
        /// </returns>
        public static Matrix OrthonormalizedRotations(Matrix m, int startRow)
        {
            OrthonormalizeRotations(ref m, startRow);
            return m;
        }

        public static void CreateMatrixFromNormal(ref Vector3 normal, out Matrix result)
        {
            bool lookingUp = false;

            Vector3 right = new Vector3(normal.Y, -normal.X, 0.0f);
            if (right.X == 0.0f && right.Y == 0.0f)
            {
                lookingUp = true;
                right.X = 1.0f;
            }

            Vector3 up;
            Vector3.Cross(ref right, ref normal, out up);

            Vector3 zeroVector = Vector3.Zero;
            if (lookingUp)
                MatrixFromRowVectors(ref right, ref up, ref normal, ref zeroVector, out result);
            else
                MatrixFromRowVectors(ref up, ref right, ref normal, ref zeroVector, out result);

            //It would be better to orthogonolize the matrix but normalizing it
            //seems to work well enough
            Normalize(ref result);
        }

        /// <summary>
        /// Creates a matrix that transforms points to lay flat along the specified plane.
        /// This is typically used to project shadows onto a plane
        /// </summary>
        /// <param name="plane">The plane onto which shadows are projected</param>
        /// <param name="light">
        /// The light position (if the light is an omni/point light) or direction (if the light is
        /// directional).
        /// </param>
        /// <param name="lightType">
        /// Indicates the type of light. 
        /// If 0, the light is a directional light.
        /// If 1, the light is an omni/point light. 
        /// </param>
        /// <returns>A matrix that projects points onto the specified plane</returns>
        public static void CreateShadowMatrix(ref Plane plane, ref Vector3 light, float lightType, out Matrix result)
        {
            //If the lightType is 0, the ray from the origin to the light represents a directional light. 
            //If it is 1, the light is a point light.

            float d = Dot(ref plane.Normal, ref light);

            float planeA = plane.Normal.X;
            float planeB = plane.Normal.Y;
            float planeC = plane.Normal.Z;

            result = new Matrix
                (
                planeA * light.X + d, planeA * light.Y, planeA * light.Z, planeA * lightType,
                planeB * light.X, planeB * light.Y + d, planeB * light.Z, planeB * lightType,
                planeC * light.X, planeC * light.Y, planeC * light.Z + d, planeC * lightType,
                plane.D * light.X, plane.D * light.Y, plane.D * light.Z, plane.D * lightType + d
                );
        }

        /// <summary>
        /// To be used with the ShadowMatrix() function as the lightType parameter
        /// </summary>
        public const float DirectShadowLightType = 0.0f;

        /// <summary>
        /// To be used with the ShadowMatrix() function as the lightType parameter
        /// </summary>
        public const float OmniShadowLightType = 1.0f;

        public static void CreateZUpCameraMatrix(ref Vector3 position, ref Vector3 target, float roll, out Matrix result)
        {
            Vector3 forward;
            Vector3.Subtract(ref position, ref target, out forward); //Yes, this order is intentional
            forward.Normalize();

            Vector3 up;
            Vector3 right;
            if (Math.Abs(Dot(ref forward, ref UnitZ)) > .99f)
            {
                //Nearly pointing up or down
                right = Vector3.UnitX;
                Vector3.Cross(ref forward, ref right, out up);
            }
            else
            {
                up = Vector3.UnitZ;
                Vector3.Cross(ref up, ref forward, out right);
            }

            Matrix orientationMatrix;
            Vector3 zeroVector = Vector3.Zero;
            MatrixFromRowVectors(ref right, ref up, ref forward, ref zeroVector, out orientationMatrix);

            Matrix rollMatrix;
            Matrix.CreateFromAxisAngle(ref forward, roll, out rollMatrix);
            Matrix.Multiply(ref orientationMatrix, ref rollMatrix, out result);
            result.Translation = position;

            OrthonormalizeRotations(ref result, 2);
        }

        public static Matrix CreatePerspectiveFovLH(float fovY, float aspect, float zn, float zf)
        {
            /*
            Generates the following matrix:
            xScale  0         0              0
            0       yScale    0              0
            0       0         zf/(zf - zn)   1
            0       0         -zn*zf/(zf-zn) 0
            */

            Matrix m = Matrix.Identity;

            float yScale = 1 / (float)Math.Tan(fovY / 2);
            float xScale = yScale / aspect;

            m.M11 = xScale;
            m.M22 = yScale;
            m.M33 = zf / (zf - zn);
            m.M34 = 1.0f;
            m.M43 = -zn * zf / (zf - zn);
            m.M44 = 0.0f;

            return m;
        }

        public static Matrix CreatePerspectiveLH(float w, float h, float zn, float zf)
        {
            /*
            Generates the following matrix:
            2*zn/w  0       0              0
            0       2*zn/h  0              0
            0       0       zf/(zf-zn)     1
            0       0       zn*zf/(zn-zf)  0
            */

            Matrix m = Matrix.Identity;

            m.M11 = 2 * zn / w;
            m.M22 = 2 * zn / h;
            m.M33 = zf / (zf - zn); m.M34 = 1;
            m.M43 = zn * zf / (zn - zf);

            return m;
        }

        public static Matrix CreateOrthoLH(float w, float h, float zn, float zf)
        {
            /*
            Generates the following matrix:
            2/w  0    0           0
            0    2/h  0           0
            0    0    1/(zf-zn)   0
            0    0    zn/(zn-zf)  1
            */

            Matrix m = Matrix.Identity;

            m.M11 = 2 / w;
            m.M22 = 2 / h;
            m.M33 = 1 / (zf - zn);
            m.M43 = zn / (zn - zf);

            return m;
        }

        public static Matrix CreateLookAtLH(Vector3 eye, Vector3 at, Vector3 up)
        {
            Vector3 x, y, z;

            //Create the z axis by normalizing the vector that goes from the eye to at
            z = at - eye;
            z.Normalize();

            //Find the x axis by normalizing the cross product of the local and world z axes
            x = Vector3.Cross(up, z);
            x.Normalize();

            //Find the y axis by normalizing the cross product of the z and x axes	
            y = Vector3.Cross(z, x);
            y.Normalize();

            //Done
            return new Matrix
                (
                x.X, x.Y, x.Z, 0,
                y.X, y.Y, y.Z, 0,
                z.X, z.Y, z.Z, 0,
                eye.X, eye.Y, eye.Z, 1
                );
        }

        public static Matrix CreateOrthoOffCenterLH(float l, float r, float b, float t, float zn, float zf)
        {
            /*
            Generates the following matrix:
            2/(r-l)      0            0           0
            0            2/(t-b)      0           0
            0            0            1/(zf-zn)   0
            (l+r)/(l-r)  (t+b)/(b-t)  zn/(zn-zf)  l
            */
            Matrix m = Matrix.Identity;

            m.M11 = 2 / (r - 1);
            m.M22 = 2 / (t - b);
            m.M33 = 1 / (zf - zn);
            m.M41 = (l + r) / (l - r); m.M42 = (t + b) / (b - t); m.M43 = zn / (zn - zf);

            return m;
        }

        /// <summary>
        /// Calculates the outer product of the specified vector
        /// </summary>
        /// <param name="v">The vector</param>
        /// <returns>The outer product</returns>
        public static Matrix OuterProduct(Vector3 v)
        {
            Matrix m;
            OuterProduct(ref v, out m);
            return m;
        }

        public static void OuterProduct(ref Vector3 v, out Matrix result)
        {
            float xy = v.X * v.Y;
            float xz = v.X * v.Z;
            float yz = v.Y * v.Z;

            result = new Matrix
                (
                v.X * v.X, xy, xz, 0.0f,
                xy, v.Y * v.Y, yz, 0.0f,
                xz, yz, v.Z * v.Z, 0.0f,
                0.0f, 0.0f, 0.0f, 1.0f
                );
        }

        /// <summary>
        /// Creates a scale matrix that scales in the specified direction by some amount
        /// </summary>
        /// <param name="direction">The direction in which to scale</param>
        /// <param name="scale">The scale factor. This value may be negative.</param>
        /// <returns>The scale matrix that scales in the specified direction</returns>
        public static Matrix CreateDirectionScaleMatrix(Vector3 direction, float scale)
        {
            Matrix m;
            CreateDirectionScaleMatrix(ref direction, scale, out m);
            return m;
        }

        public static void CreateDirectionScaleMatrix(ref Vector3 direction, float scale, out Matrix result)
        {
            CreateDirectionScaleMatrix(ref direction, scale, scale, scale, out result);
        }

        public static Matrix CreateDirectionScaleMatrix(Vector3 direction, float scaleX, float scaleY, float scaleZ)
        {
            Matrix m;
            CreateDirectionScaleMatrix(ref direction, scaleX, scaleY, scaleZ, out m);
            return m;
        }

        public static void CreateDirectionScaleMatrix(ref Vector3 direction, float scaleX, float scaleY, float scaleZ, out Matrix result)
        {
            float xy = direction.X * direction.Y;
            float xz = direction.X * direction.Z;
            float yz = direction.Y * direction.Z;

            result = new Matrix
                (
                1.0f + (scaleX - 1.0f) * direction.X * direction.X, (scaleY - 1.0f) * xy                              , (scaleZ - 1.0f) * xz                              , 0.0f,
                (scaleX - 1.0f) * xy                              , 1.0f + (scaleY - 1.0f) * direction.Y * direction.Y, (scaleZ - 1.0f) * yz                              , 0.0f,
                (scaleX - 1.0f) * xz                              , (scaleY - 1.0f) * yz                              , 1.0f + (scaleZ - 1.0f) * direction.Z * direction.Z, 0.0f,
                0.0f                                              , 0.0f                                              , 0.0f                                              , 1.0f
                );
        }

        /// <summary>
        /// Creates a matrix that reflects to the other side of the specified plane
        /// </summary>
        /// <param name="plane">The plane</param>
        /// <returns>The matrix that reflects to the other side of the specified plane</returns>
        public static Matrix CreateReflectionMatrix(Plane plane)
        {
            Matrix m;
            CreateReflectionMatrix(ref plane, out m);
            return m;
        }

        public static void CreateReflectionMatrix(ref Plane plane, out Matrix result)
        {
            //Scale to the other side of the plane
            CreateDirectionScaleMatrix(ref plane.Normal, -1.0f, out result);

            //Translate to the other side of the plane, moving along the 
            //negative direction of the plane normal
            result.Translation = plane.Normal * -2.0f * plane.D;
        }

        /// <summary>
        /// Calculates the sum of all the elements in the specified matrix
        /// </summary>
        /// <param name="m">The matrix</param>
        /// <returns>The sum of all elements in the specified matrix</returns>
        public static float Sum(ref Matrix m)
        {
            return
                m.M11 + m.M12 + m.M13 + m.M14 +
                m.M21 + m.M22 + m.M23 + m.M24 +
                m.M31 + m.M32 + m.M33 + m.M34 +
                m.M41 + m.M42 + m.M43 + m.M44;
        }

        #endregion

        #region Quaternion functions

        /// <summary>
        /// Extracts the axis and angle from a quaternion
        /// </summary>
        /// <param name="q">The quaternion</param>
        /// <param name="axis">The output axis</param>
        /// <param name="angle">The output angle</param>
        public static void ToAxisAngle(ref Quaternion q, out Vector3 axis, out float angle)
        {
            angle = (float)Math.Acos(q.W);
            float ooScale = 1.0f / Sin(angle);
            angle *= 2.0f;

            axis = new Vector3(-q.X * ooScale, -q.Y * ooScale, -q.Z * ooScale);
        }

        #endregion

        #region Plane functions

        /// <summary>
        /// Calculates the distance from the point to the plane
        /// </summary>
        /// <param name="plane">The plane</param>
        /// <param name="point">The point</param>
        /// <returns>The distance from the point to the plane</returns>
        public static float DistanceToPoint(ref Plane plane, ref Vector3 point)
        {
            return plane.Normal.X * point.X + plane.Normal.Y * point.Y + plane.Normal.Z * point.Z + plane.D;
        }

        /// <summary>
        /// Normalizes the specified plane as a 4-vector.
        /// </summary>
        /// <param name="plane">The plane to normalize</param>
        public static void Normalize(ref Plane plane)
        {
            float ooLength = 1.0f / Sqrt
                (
                plane.Normal.X * plane.Normal.X +
                plane.Normal.Y * plane.Normal.Y +
                plane.Normal.Z * plane.Normal.Z +
                plane.D * plane.D
                );

            plane.Normal.X *= ooLength;
            plane.Normal.Y *= ooLength;
            plane.Normal.Z *= ooLength;
            plane.D *= ooLength;
        }

        /// <summary>
        /// Negates all the coefficients of the specified plane
        /// </summary>
        /// <param name="plane">The plane to negate</param>
        public static void Negate(ref Plane plane)
        {
            plane.Normal.X = -plane.Normal.X;
            plane.Normal.Y = -plane.Normal.Y;
            plane.Normal.Z = -plane.Normal.Z;
            plane.D = -plane.D;
        }

        /// <summary>
        /// Transforms the plane coefficients as if they were a Vector4
        /// </summary>
        public static void TransformPlaneAsVector4(ref Plane plane, ref Matrix m, out Plane result)
        {
            result = new Plane
                (
                plane.Normal.X * m.M11 + plane.Normal.Y * m.M21 + plane.Normal.Z * m.M31 + plane.D * m.M41,
                plane.Normal.X * m.M12 + plane.Normal.Y * m.M22 + plane.Normal.Z * m.M32 + plane.D * m.M42,
                plane.Normal.X * m.M13 + plane.Normal.Y * m.M23 + plane.Normal.Z * m.M33 + plane.D * m.M43,
                plane.Normal.X * m.M14 + plane.Normal.Y * m.M24 + plane.Normal.Z * m.M34 + plane.D * m.M44
                );
        }

        /// <summary>
        /// Subtracts all the values in one plane from another
        /// </summary>
        /// <param name="p0">The plane to subtract from</param>
        /// <param name="p1">The plane that subtracts</param>
        /// <returns>The resultant plane values</returns>
        public static void Subtract(ref Plane p0, ref Plane p1, out Plane result)
        {
            result = new Plane(p0.Normal.X - p1.Normal.X, p0.Normal.Y - p1.Normal.Y, p0.Normal.Z - p1.Normal.Z, p0.D - p1.D);
        }

        /// <summary>
        /// Adds all the values in one plane to another
        /// </summary>
        /// <param name="p0">The first plane</param>
        /// <param name="p1">The other plane</param>
        /// <returns>The resultant plane values</returns>
        public static void Add(ref Plane p0, ref Plane p1, out Plane result)
        {
            result = new Plane(p0.Normal.X + p1.Normal.X, p0.Normal.Y + p1.Normal.Y, p0.Normal.Z + p1.Normal.Z, p0.D + p1.D);
        }

        /// <summary>
        /// Creates a plane from three points in a triangle. This exists as a fix
        /// to the Game Studio Express beta 2 which broke the Plane functionality
        /// that worked fine in beta 1. The strange thing is that I have a hard time
        /// duplicating the bug in an isolated environment. But then, I haven't tried
        /// very hard either
        /// </summary>
        /// <param name="p0">The first point</param>
        /// <param name="p1">The second point</param>
        /// <param name="p2">The third point</param>
        /// <returns>The plane that contains the three triangle points</returns>
        public static void CreatePlane(ref Vector3 p0, ref Vector3 p1, ref Vector3 p2, out Plane result)
        {
            Vector3 normal;
            CreateNormal(ref p0, ref p1, ref p2, out normal);

            result = new Plane(normal, -Dot(ref normal, ref p0));
        }

        /// <summary>
        /// Calculates the point where three planes intersect
        /// </summary>
        /// <param name="point">The output intersection point where the three planes intersect</param>
        /// <param name="p0">The first plane</param>
        /// <param name="p1">The second plane</param>
        /// <param name="p2">The third plane</param>
        /// <returns>true if the planes intersected, false otherwise</returns>
        public static bool IntersectThreePlanes(ref Plane p0, ref Plane p1, ref Plane p2, out Vector3 point)
        {
            Vector3 n1_n2;
            Vector3.Cross(ref p1.Normal, ref p2.Normal, out n1_n2);

            Vector3 n2_n0;
            Vector3.Cross(ref p2.Normal, ref p0.Normal, out n2_n0);

            Vector3 n0_n1;
            Vector3.Cross(ref p0.Normal, ref p1.Normal, out n0_n1);

            float cosTheta = Dot(ref p0.Normal, ref n1_n2);

            if (AlmostZero(cosTheta) || float.IsInfinity(cosTheta))
            {
                point = Vector3.Zero;
                return false;
            }

            float secTheta = 1.0f / cosTheta;

            n1_n2 *= p0.D;
            n2_n0 *= p1.D;
            n0_n1 *= p2.D;

            point = (n1_n2 + n2_n0 + n0_n1) * -secTheta;
            return true;
        }

        #endregion

        #region Bounding box functions

        /// <summary>
        /// Gets the center of the specified bounding box
        /// </summary>
        /// <param name="box">The box whose center is obtained</param>
        /// <returns>The bounding box center</returns>
        public static void GetBoundingBoxCenter(ref BoundingBox box, out Vector3 center)
        {
            Vector3.Add(ref box.Min, ref box.Max, out center);
            center.X /= 2;
            center.Y /= 2;
            center.Z /= 2;
        }

        /// <summary>
        /// Gets the dimensions of the specified bounding box
        /// </summary>
        /// <param name="box">The box whose dimensions are obtained</param>
        /// <returns>The bounding box dimensions</returns>
        public static void GetBoundingBoxSize(ref BoundingBox box, out Vector3 size)
        {
            Vector3.Subtract(ref box.Max, ref box.Min, out size);
        }

        /// <summary>
        /// Transforms the specified bounding box by a matrix, yielding a new box.
        /// </summary>
        /// <param name="matrix">The matrix used to transform the box</param>
        /// <param name="box">
        /// The box to transform. This is a reference value, but the original box is 
        /// not changed
        /// </param>
        /// <returns>A new, transformed version of the input box</returns>
        public static void TransformedBoundingBox
            (
            ref Matrix matrix,
            ref BoundingBox box,
            out BoundingBox result
            )
        {
            Vector3[] corners = box.GetCorners();
            TransformArray(corners, ref matrix);
            result = BoundingBox.CreateFromPoints(corners);
        }

        /// <summary>
        /// Creates a bounding box from the specified points. This differs from
        /// BoundingBox.CreateFromPoints() by allowing an empty collection of points.
        /// </summary>
        /// <param name="points">The points from which the bounding box is created</param>
        /// <returns>A bounding box that bounds all of the specified points</returns>
        public static void BoundingBoxFromPoints(IEnumerable<Vector3> points, out BoundingBox result)
        {
            if (points.GetEnumerator().MoveNext())
                result = BoundingBox.CreateFromPoints(points);
            else
                result = new BoundingBox();
        }

        /// <summary>
        /// Creates a bounding box from the specified array of points. This differs
        /// from BoundingBox.CreateFromPoints() by taking an array and number of items
        /// to use in the array
        /// </summary>
        /// <param name="points">The points from which the bounding box is created.</param>
        /// <param name="length">
        /// The number of points to use in the array, starting
        /// from the first point.
        /// </param>
        /// <returns>A bounding box that bounds all of the specified points</returns>
        public static void BoundingBoxFromPoints(Vector3[] points, int length, out BoundingBox result)
        {
            result = new BoundingBox();
            result.Min = points[0];
            result.Max = points[0];

            for (int index = 1; index < length; index++)
            {
                result.Min.X = Math.Min(result.Min.X, points[index].X);
                result.Min.Y = Math.Min(result.Min.Y, points[index].Y);
                result.Min.Z = Math.Min(result.Min.Z, points[index].Z);

                result.Max.X = Math.Max(result.Max.X, points[index].X);
                result.Max.Y = Math.Max(result.Max.Y, points[index].Y);
                result.Max.Z = Math.Max(result.Max.Z, points[index].Z);
            }
        }

        /// <summary>
        /// Creates a bounding box by merging all the input bounding boxes
        /// </summary>
        /// <param name="boxes">A collection of bounding boxes</param>
        /// <returns>A bounding box by merging all the input bounding boxes</returns>
        public static void MergedBoundingBoxes(IEnumerable<BoundingBox> boxes, out BoundingBox result)
        {
            IEnumerator<BoundingBox> item = boxes.GetEnumerator();
            if (item.MoveNext())
            {
                //Initialize with the first bounding box
                result = item.Current;

                //Move past the first item, then get the rest
                while (item.MoveNext())
                    result = BoundingBox.CreateMerged(result, item.Current);
            }
            else
                result = new BoundingBox();
        }

        /// <summary>
        /// Creates a bounding box by merging all the input bounding boxes
        /// </summary>
        /// <param name="boxes">A collection of bounding boxes</param>
        /// <param name="length">
        /// The number of boxes to use in the array, starting from the first box.
        /// </param>
        /// <returns>A bounding box by merging all the input bounding boxes</returns>
        public static void MergedBoundingBoxes(BoundingBox[] boxes, int length, out BoundingBox result)
        {
            if (length > 0)
            {
                result = boxes[0];

                for (int index = 1; index < length; index++)
                    result = BoundingBox.CreateMerged(result, boxes[index]);
            }
            else
                result = new BoundingBox();
        }

        /// <summary>
        /// Adds the point to the bounding box, if the point is not in the box
        /// </summary>
        /// <param name="box">The bounding box to add to.</param>
        /// <param name="point">The point to add to the bounding box</param>
        public static void MergeBoundingBoxPoint(ref BoundingBox box, ref Vector3 point)
        {
            box.Min.X = Math.Min(box.Min.X, point.X);
            box.Min.Y = Math.Min(box.Min.Y, point.Y);
            box.Min.Z = Math.Min(box.Min.Z, point.Z);

            box.Max.X = Math.Max(box.Max.X, point.X);
            box.Max.Y = Math.Max(box.Max.Y, point.Y);
            box.Max.Z = Math.Max(box.Max.Z, point.Z);
        }

        public static bool IntersectRayWithBoundingBox
            (
            BoundingBox box,
            out float hitDistance,
            Vector3 origin,
            Vector3 direction
            )
        {
            Plane[] sides = new Plane[]
                { 
                new Plane(1, 0, 0,-box.Min.X), new Plane(-1, 0, 0, box.Max.X),
                new Plane(0, 1, 0,-box.Min.Y), new Plane(0,-1, 0, box.Max.Y),
                new Plane(0, 0, 1,-box.Min.Z), new Plane(0, 0,-1, box.Max.Z) 
                };

            hitDistance = 0.0f;
            Vector3 hitPoint = origin;

            bool inside = false;

            for (int index = 0; index < 6 && !inside; index++)
            {
                float cosTheta = Dot(ref sides[index].Normal, ref direction);
                float distance = DistanceToPoint(ref sides[index], ref origin);

                //If we're nearly intersecting, just punt and call it an intersection
                if (AlmostZero(distance))
                    return true;

                //Skip nearly (&actually) parallel rays
                if (AlmostZero(cosTheta))
                    continue;

                //Only interested in intersections along the ray, not before it.
                hitDistance = -distance / cosTheta;
                if (hitDistance < 0.0f)
                    continue;

                hitPoint = hitDistance * direction + origin;

                inside = true;

                for (int j = 0; (j < 6) && inside; j++)
                {
                    if (j == index)
                        continue;

                    float d = DistanceToPoint(ref sides[j], ref hitPoint);

                    inside = ((d + 0.00015f) >= 0.0f);
                }
            }

            return inside;
        }

        #endregion

        #region Sphere functions

        public static bool SweptSpherePlaneIntersect
            (
            ref Plane plane,
            ref Vector3 sphereCenter,
            float sphereRadius,
            ref Vector3 sweepDirection,
            out float t0,
            out float t1
            )
        {
            float distance = DistanceToPoint(ref plane, ref sphereCenter);
            float d_dot_n = Dot(ref plane.Normal, ref sweepDirection);

            if (Math.Abs(d_dot_n) < .0001f)
            {
                //The sphere is moving parallel to the plane

                if (distance <= sphereRadius)
                {
                    //The sphere intersects the plane infinitely along the sweep direction
                    t0 = 0.0f;
                    t1 = 1E32f;
                    return true;
                }
                else
                {
                    //There can never be a collision
                    t0 = 0;
                    t1 = 0;
                    return false;
                }
            }
            else
            {
                //The sphere is not moving parallel to the plane

                float tmp0 = (sphereRadius - distance) / d_dot_n;
                float tmp1 = (-sphereRadius - distance) / d_dot_n;
                t0 = Math.Min(tmp0, tmp1);
                t1 = Math.Max(tmp0, tmp1);
                return true;
            }
        }

        #endregion

        #region Constants

        /// <summary>
        /// A default camera matrix, with the world Z axis pointing up, and the X/Y
        /// axis on the ground plane. This is the same matrix a camera created 
        /// in the front viewport of 3D Studio Max receives.
        /// </summary>
        public static readonly Matrix ZUpCameraMatrix = new Matrix
            (
            1, 0, 0, 0,
            0, 0, 1, 0,
            0, -1, 0, 0,
            0, 0, 0, 0
            );

        //Some read-only constants that aren't marked as readonly. Don't change them!

        public static readonly Matrix IdentityMatrix = Matrix.Identity;

        public static readonly Vector3 UnitX = Vector3.UnitX;
        public static readonly Vector3 UnitY = Vector3.UnitY;
        public static  Vector3 UnitZ = Vector3.UnitZ;

        #endregion
    }
}